Adaptive Planning Horizon Based on Information Velocity for Vision-Based Navigation

This paper presents a receding horizon planning algorithm for vision-based navigation using bearings-only SLAM that adapts the planning horizon to the velocity of the information gained about the environment. Bearings-only SLAM has an inherent dynamic observability property such that specific motion is needed in order to determine the relative positions between the camera and obstacles. Thus, the estimates of new obstacles are always highly uncertain and information about the obstacles is dependent on the relative motion between the camera and objects. Receding horizon planners are typically used in conjunction with SLAM algorithms because they continually adjust to new information about the world. We present a receding horizon planner that incorporates information metrics explicitly into the receding optimization cost function. Furthermore, an adaptive horizon is used based on the intuition that when the information about the world is rapidly changing, planning does not need to be too long. Control and planning horizons are computed based on the sensor range and the effective speed of the UAV, which is computed as a weighted sum of estimated vehicle speed and time rate of change of the uncertainty of the obstacle position estimates. Simulation results demonstrate the integrated receding horizon system for vision-based SLAM; illustrate the value of integrating information-theoretic costs into the objective function and the adaptive approach; and extension of the planning approach to other applications such as exploration, target intercept, and cooperative active sensing.